// $Id: AuthenticationLayer.nc,v 1.6 2008/06/24 05:32:31 regehr Exp $

/*									tab:4
 * "Copyright (c) 2000-2005 The Regents of the University  of California.  
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 * 
 * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
 * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
 * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
 * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 *
 * Copyright (c) 2002-2003 Intel Corporation
 * All rights reserved.
 *
 * This file is distributed under the terms in the attached INTEL-LICENSE     
 * file. If you do not find these files, copies can be found by writing to
 * Intel Research Berkeley, 2150 Shattuck Avenue, Suite 1300, Berkeley, CA, 
 * 94704.  Attention:  Intel License Inquiry.
 */
 
#include "Timer.h"
#include "RadioCountToLeds.h"

#define DEST 1
#define DETECTOR 0
#define BS 1
#define DIM 10
#define MAXMSGS 20
#define DIFVAL 2
#define DIFTIME 2

#define RATELIM 9
#define PEAKLIM 2
#define GAPLIM 2


#define ATOMIC 0
#define MACE 0
#define FLOOD 1
#define SPACE 0
#define TIME 0
#define ROUTING 0

 
/**
 * @author Laura Gheorghe
 * @date   March 2012
 */

module RadioCountToLedsC @safe() {
  uses {
    	interface Boot;
    	interface Receive;
    	interface AMSend;
    	interface Timer<TMilli> as Collect_Timer;
	#if FLOOD==1
    	interface Timer<TMilli> as Flood_Timer;
    	interface Timer<TMilli> as Collect_Mal_Timer;
	#endif
    	interface SplitControl as AMControl;
    	interface Packet;
    	interface AMPacket;
  }
}
implementation {

 	message_t packet;
  	bool locked;
	uint16_t next_hop;
	uint16_t seq_no = 1;
	uint32_t payload2;

	#if SPACE==1
	int no_msg=0;
	stored_msg_t msgs[MAXMSGS];
	stored_msg_t messages[DIM][DIM];

	
	co_t coord[DIM];
	uint16_t clusters[DIM][DIM];
	uint8_t dim_cl[DIM];
	uint8_t no_cl;
	#endif


	stored_msg1_t time_mess[DIM];
	double time=1;


	#if FLOOD==1
	uint8_t first=0;
	uint16_t seconds=0;
	uint8_t rate[DIM];
	uint8_t rate_total;
	uint8_t gap[DIM];
	uint8_t gap_total;
	uint8_t peak[DIM];
	uint8_t peak_total;
	uint8_t alert[DIM];
	uint8_t alert_total;
	#endif

	#if ROUTING==1
	routing_msg_t routing[MAXMSGS];
	uint8_t no_routing=0;
	bool marked[MAXMSGS];
	#endif

	uint32_t payload3=10;

	uint8_t no_pack;
	


 	event void Boot.booted() {
		int i,j;
		am_addr_t my_addr;
    		call AMControl.start();
		my_addr=call AMPacket.address();
		if (my_addr==7) payload2=1;
		if (my_addr==4) payload2=2;
		if (my_addr==2) payload2=7;
		if (my_addr==0) payload2=8;
		if (my_addr==1) payload2=11;
		if (my_addr==3) payload2=12;
		if (my_addr==5) payload2=20;
		if (my_addr==6) payload2=14;
		if (my_addr==8) payload2=3;
		if (my_addr==9) payload2=4;
		if (my_addr==0){
			next_hop=1;
		}
		if (my_addr == 2){
			next_hop = 0;
		}
		if (my_addr==7 || my_addr==8 || my_addr==9){
			next_hop=4;
		}
		if (my_addr==4){
			next_hop=2;
		}
		
		if (my_addr==3){
			next_hop=0;
		}
	
		if (my_addr==5 || my_addr==6){
			next_hop=3;
		}
	
		#if FLOOD==1
		for (i=0;i<DIM;i++){
			time_mess[i].time=0;
			alert[i]=0;
			rate[i]=0;
			gap[i]=0;
			peak[i]=0;
		}
		#endif

		#if SPACE==1
		coord[7].x=0; coord[7].y=7;
		coord[4].x=1; coord[4].y=7;
		coord[2].x=1; coord[2].y=3;
		coord[0].x=1; coord[0].y=0;
		coord[1].x=0; coord[1].y=0;
		coord[3].x=8; coord[3].y=0;
		coord[5].x=9; coord[5].y=0;
		coord[6].x=8; coord[6].y=2;
		coord[8].x=2; coord[8].y=8;
		coord[9].x=3; coord[9].y=7;
		clusters[0][0]=4;
		clusters[0][1]=7;
		clusters[0][2]=8;
		clusters[0][3]=9;
		dim_cl[0]=4;
		clusters[1][0]=2;
		clusters[1][1]=0;
		clusters[1][2]=1;
		dim_cl[1]=3;
		clusters[2][0]=3;
		clusters[2][1]=5;
		clusters[2][2]=6;
		dim_cl[2]=3;
		no_cl=3;
		#endif
  	}

	

	uint32_t MAC(uint32_t c_payload, uint32_t p_payload, uint8_t seq){
		
		uint32_t payload_all=c_payload+p_payload+KEY+seq;
		uint32_t c=129-payload_all*6;
		int i=0;
		for (i=0;i<32;i++){
			if ((payload_all&1)!=0) c--;
			payload_all=payload_all>>1;
		}
		return c;
	}

		
	#if ATOMIC==1
	uint8_t atomic_engine(radio_count_msg_t* rcm){
		//check atomic signatures
		uint8_t blk=0;	
		am_addr_t my_addr=call AMPacket.address();

		if (rcm->end_dest != BS){
			dbg("TinyAMF", "[%f] TinyAMF -> Atomic engine: End destination address invalid. Packet is blocked.[end_src=%hu end_dest=%hu seq=%hu payload=%d] \n", time,  rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
			blk=1;
		}
		if (rcm->end_src < 0 || rcm->end_src > 9){
			dbg("TinyAMF", "[%f] TinyAMF -> Atomic engine: End source address invalid. [end_src=%hu end_dest=%hu seq=%hu payload=%d] \n", time,  rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
			blk=1;
		}
		return blk;
	}
	#endif
	

	#if MACE==1
	uint8_t MAC_engine(radio_count_msg_t* rcm){
	
		uint8_t blk=0;

		if (rcm->MAC != MAC(rcm->payload, 0, rcm->seq)){
			dbg("TinyAMF", "[%f] TinyAMF -> MAC engine: Invalid MAC. [end_src=%hu end_dest=%hu seq=%hu payload=%d] \n", time,  rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
			blk=1;
		}	
		return blk;
	}
	#endif


	#if SPACE==1
	void save_packet(radio_count_msg_t* rcm){
		msgs[no_msg].payload = rcm->payload;
		msgs[no_msg].end_src = rcm->end_src;
		msgs[no_msg].src_x = rcm->src_x;
		msgs[no_msg].src_y = rcm->src_y;
		msgs[no_msg].seq = rcm->seq;
		msgs[no_msg].end_dest = rcm->end_dest;
		no_msg++;
		//dbg("TinyAMF", "[%f] TinyAMF -> Space correlation engine: Saved packet from end_src=%hu src_x=%hu src_y=%hu payload=%d no_msg=%d\n", time, rcm->end_src, rcm->src_x, rcm->src_y, rcm->payload,no_msg);
	}
	#endif

	int poz(int x){
		if (x<0) return -x;
	}

	#if TIME==1
	uint8_t time_correlation_engine(radio_count_msg_t* rcm){
		uint8_t blk=0;
		int i,j;

		
		if (time_mess[rcm->end_src].time!=0 && poz(time_mess[rcm->end_src].payload-rcm->payload)>DIFVAL && poz(time_mess[rcm->end_src].time-time)<DIFTIME){
			dbg("TinyAMF", "[%f] TinyAMF -> Time correlation engine: Suspicious data. [end_src=%hu end_dest=%hu seq=%hu payload=%d] \n", time,  rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
			blk=1;
		}

		time_mess[rcm->end_src].payload = rcm->payload;
		time_mess[rcm->end_src].time = time;
		
		return blk;
	}
	#endif

	#if SPACE==1
	void space_correlation_engine(){

		uint8_t i,j,k,newm,no_m;
		stored_msg_t aux;
		uint32_t med;
		uint8_t no_msgs[DIM];	
		stored_msg_t bad[DIM];
		uint16_t difb[DIM];
		uint8_t no_bad=0;
			
		for (j=0;j<no_cl;j++){
			no_m=0;
			for (k=0;k<dim_cl[j];k++){
				for (i=0;i<no_msg;i++){
					if (clusters[j][k]==msgs[i].end_src){
							messages[j][no_m]=msgs[i];
							//dbg("RadioCountToLedsC", "cluster=%d msg=%d src=%d\n",j, messages[j][no_m].payload, messages[j][no_m].end_src);
							no_m++;
					}
				}
			}
			no_msgs[j]=no_m;
		}
		

		for (j=0;j<no_cl;j++){
			while (TRUE){
				newm=0;
				for (k=0;k<(no_msgs[j]-1);k++){
					if (messages[j][k].payload>messages[j][k+1].payload){
						aux=messages[j][k];
						messages[j][k]=messages[j][k+1];
						messages[j][k+1]=aux;
						newm=1;
					}

				}
				if (newm==0) break;
			}
			for (k=0;k<no_msgs[j];k++){
				//dbg("RadioCountToLedsC", "after cluster=%d msg=%d src=%d\n",j, messages[j][k].payload, messages[j][k].end_src);
			}
		}
		for (j=0;j<no_cl;j++){
			if (no_msgs[j]<3) continue;
			med=messages[j][no_msgs[j]/2].payload;
			//dbg("RadioCountToLedsC", "cluster=%d med=%d val_med=%d\n",j,no_msgs[j]/2, med);
			for (k=0;k<no_msgs[j];k++){
				if ((poz(messages[j][k].payload-med))>DIFVAL){
					bad[no_bad]=messages[j][k];	
					difb[no_bad]=poz(messages[j][k].payload-med);
					no_bad++;
				}
			}
		}
		for (i=0;i<no_bad;i++){
			dbg("TinyAMF", "[%f] TinyAMF -> Space correlation engine: Suspicious data. [end_src=%d end_dest=%hu seq=%hu payload=%hu]\n", time, bad[i].end_src, bad[i].end_dest, bad[i].seq, bad[i].payload);
		}	
		no_msg=0;

	}
	#endif

	#if FLOOD==1
	event void Flood_Timer.fired() {
		
		short unsigned i=0;
		seconds++;
		for (i=0;i<DIM;i++){
			if (rate[i]<RATELIM){
				gap[i]++;
				rate[i]=0;
				//if (alert[i]==0)
					//dbg("TinyAMF", "Flood engine: Rate=%u Peak=%u Gap=%u\n",rate[i], peak[i], gap[i]);
				if (gap[i]>=GAPLIM){
					peak[i]=0;
				}
			}
			else{
				rate[i]=0;
				peak[i]++;			
				gap[i]=0;
			}
		}

		/*if (rate_total<RATELIM){
			gap_total++;
			rate_total=0;
			if (alert_total==0)
				//dbg("TinyAMF", "Flood engine: Rate=%u Peak=%u Gap=%u\n",rate_total, peak_total, gap_total);
			if (gap_total>=GAPLIM){
				peak_total=0;
			}
		}
		else{
			rate_total=0;
			peak_total++;			
			gap_total=0;
		}*/
		
		
  	}

	
  	void flood_engine(radio_count_msg_t* rcm){

		if (alert[rcm->end_src] == 0){

			if (first==0){
				first=1;
				call Flood_Timer.startPeriodic(1000);
			}
			rate[rcm->end_src]=rate[rcm->end_src]+1;
			//rate_total=rate_total+1;
 
			//dbg("TinyAMF", "[%f] Packet received Rate=%u Peak=%u Gap=%u (src=%hu dest=%hu msg=%d)\n", time, rate[rcm->end_src], peak[rcm->end_src], gap[rcm->end_src], rcm->end_src, rcm->end_dest, rcm->payload);
			if ((rate[rcm->end_src]>RATELIM && peak[rcm->end_src]>(PEAKLIM-1)) || peak[rcm->end_src]>=PEAKLIM){
				alert[rcm->end_src]=1;
				dbg("TinyAMF", "[%f] TinyAMF -> Flood engine: Rate exceeded. Rate=%u Peak=%u Gap=%u. [end_src=%hu end_dest=%hu seq=%hu payload=%d]\n", time, rate[rcm->end_src], peak[rcm->end_src], gap[rcm->end_src], rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
			}
			/*if ((rate_total>RATELIM && peak_total>(PEAKLIM-1)) || peak_total>=PEAKLIM){
				alert_total=1;
				dbg("TinyAMF", "[%f] Flood engine: Alert, rate exceeded. Rate=%u Peak=%u Gap=%u (dest=%hu msg=%d)\n", time, rate_total, peak_total, gap_total, rcm->end_dest, rcm->payload);
			}*/
		}
  	}
	#endif

	#if ROUTING==1
	void delete_routing(uint8_t i){
		uint8_t j;

		for (j=i;j<no_routing;j++){
			routing[j]=routing[j+1];
			marked[j]=marked[j+1];
		}
		no_routing--;
	}

	void add_routing(radio_count_msg_t* rcm){
		routing[no_routing].end_src = rcm->end_src;
		routing[no_routing].end_dest = rcm->end_dest;
		routing[no_routing].src = rcm->src;
		routing[no_routing].dest = rcm->dest;
		routing[no_routing].seq = rcm->seq;
		routing[no_routing].payload = rcm->payload;
		marked[no_routing] = FALSE;
		no_routing++;
	}
	void print_routing(){
		uint8_t i;
		for (i=0;i<no_routing;i++){
			dbg("TinyAMF", "[%f] Routing engine: end_src=%d src=%d dest=%d seq=%hu marked=%d\n", time,routing[i].end_src,routing[i].src, routing[i].dest, routing[i].seq, marked[i]);
		}
		dbg("TinyAMF", "\n");
	}
	

	void routing_engine(radio_count_msg_t* rcm){
		bool gasit=FALSE;
		uint8_t i=0, t_no=0, b_no=0, del=0;
		am_addr_t my_addr=call AMPacket.address();
		am_addr_t target=rcm->dest;
		
		//if (target != my_addr)
		//	dbg("TinyAMF", "[%f] Routing engine: [end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d]\n", time, rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);
		for (i=0;i<no_routing;i++){
			if ((routing[i].seq==rcm->seq) && (routing[i].end_src==rcm->end_src) && (routing[i].end_dest==rcm->end_dest) && (routing[i].payload==rcm->payload)){
				gasit = TRUE;
				marked[i] = TRUE;
				target = routing[i].dest;
				//if (routing[i].end_src==5) dbg("TinyAMF", "[%f] Routing engine: mark end_src=%d src=%d dest=%d seq=%hu marked=%d\n", time,routing[i].end_src,routing[i].src, routing[i].dest, routing[i].seq, marked[i]);
			}
		}
		if (gasit == FALSE){
			add_routing(rcm);
		}
		
		for (i=0;i<no_routing;i++){
			if (routing[i].dest==target){ 
				t_no++;
				if(marked[i]==FALSE) b_no++;
			}
		}
		//dbg("TinyAMF", "[%f] no_routing=%hu t_no=%hu b_no=%hu\n", time,no_routing, t_no,b_no);
	
		if ((b_no == t_no) && (b_no >= 10)){ 
			dbg("TinyAMF", "[%f] Routing engine: Blackhole attack [end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d]\n", time, rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);
			for (i = 0;i < no_routing;i++){
				if (routing[i].dest == target){
					delete_routing(i);
				}
			}
			
		}
		else
		if ((b_no < t_no) && (b_no >= 10)){ 
			dbg("TinyAMF", "[%f] Routing engine: Selective Forwarding attack [end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d]\n", time, rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);
			for (i = 0;i < no_routing;i++){
				if (routing[i].dest == target){
					delete_routing(i);
				}
			}
			
		}
		else
		if (t_no > 20){
			for (i = 0;i < no_routing;i++){
				if (routing[i].dest == target){
					delete_routing(i);
				}
			}
			
		}
		//print_routing();

	}
	#endif
 #if FLOOD==1
 event void Collect_Mal_Timer.fired() {
	
	am_addr_t my_addr=call AMPacket.address();
	am_addr_t dest;


	radio_count_msg_t* rcm = (radio_count_msg_t*)call Packet.getPayload(&packet, sizeof(radio_count_msg_t));

	
      		if (rcm == NULL) {
			return;
     	 	}

		#if SPACE==1
		rcm->src_x=coord[my_addr].x;
		rcm->src_y=coord[my_addr].y;
		#endif

      		rcm->payload = payload2;
		rcm->src = my_addr;
		rcm->dest = next_hop;
		rcm->end_src = my_addr;
		rcm->end_dest = BS;
		rcm->seq = seq_no;

		rcm->MAC = MAC(rcm->payload,0, rcm->seq);
		
		
	      	if (call AMSend.send(next_hop, &packet, sizeof(radio_count_msg_t)) == SUCCESS) {
			//dbg("RadioCountToLedsC", "[%f] Packet sent (end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d)\n",  time, rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);		
			locked = TRUE;
	      	}

		seq_no++;
	
  }
  #endif
 
  event void Collect_Timer.fired() {
	am_addr_t my_addr=call AMPacket.address();
	am_addr_t dest;

	//if (seq_no > 3) return;

	//time = (double)sim_time()/sim_ticks_per_sec();

	//dbg("TinyAMF", "time=%f\n", time);


	if (my_addr == DETECTOR){ 
		
		
		#if SPACE==1
		space_correlation_engine();
		#endif
	}

	if (my_addr == 0 || my_addr == 1) return;

	#if TIME==1
	if (my_addr == 8 && seq_no == 2){ 
		payload2 = payload2 + 3;
	}
	else
		payload2++;
	#else
	payload2++;
	#endif

	if (locked) {
      		return;
    	}
    	else {
		radio_count_msg_t* rcm = (radio_count_msg_t*)call Packet.getPayload(&packet, sizeof(radio_count_msg_t));
	
      		if (rcm == NULL) {
			return;
     	 	}

		#if SPACE==1
		rcm->src_x=coord[my_addr].x;
		rcm->src_y=coord[my_addr].y;
		#endif

		//dest=r[src];
	
      		rcm->payload = payload2;
		rcm->src = my_addr;
		rcm->dest = next_hop;
		rcm->end_src = my_addr;
		rcm->end_dest = BS;
		rcm->seq = seq_no;

		

		#if MAC==1
		if (rcm->seq == 2 && rcm->end_src == 9)
			rcm->MAC = MAC(rcm->payload,1, rcm->seq);
		else 
			rcm->MAC = MAC(rcm->payload,0, rcm->seq);
		#else
			rcm->MAC = MAC(rcm->payload,0, rcm->seq);
		#endif

		#if ATOMIC==1
		if (rcm->seq == 1 && rcm->end_src == 6)
			rcm->end_src=20;
		#endif
		
	      	if (call AMSend.send(next_hop, &packet, sizeof(radio_count_msg_t)) == SUCCESS) {
			//if (my_addr == 9 ) //& seq_no == 40
			//if (next_hop == 3)
			//	dbg("RadioCountToLedsC", "[%f] Packet sent (end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d)\n", time,  rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);		
			locked = TRUE;
	      	}

		seq_no++;
	}
	
  }



  event void AMControl.startDone(error_t err) {
	am_addr_t my_addr=call AMPacket.address();
    if (err == SUCCESS) {
	//time = (double)sim_time()/sim_ticks_per_sec();
	//dbg("TinyAMF", "time=%f\n", time);
      	call Collect_Timer.startPeriodic(1000);
	#if FLOOD==1
	if (my_addr == 2) call Collect_Mal_Timer.startPeriodic(100);
	#endif
    }
    else {
      call AMControl.start();
    }
  }

  event void AMControl.stopDone(error_t err) {
    // do nothing
  }

  event message_t* Receive.receive(message_t* bufPtr, 
				   void* payload, uint8_t len) {
	
    if (len != sizeof(radio_count_msg_t)) {return bufPtr;}
    else {
      radio_count_msg_t* rcm = (radio_count_msg_t*)payload;
	am_addr_t src=call AMPacket.source(bufPtr);
	uint32_t challange;
	uint8_t block;
	am_addr_t my_addr=call AMPacket.address();
	

	radio_count_msg_t* rcm1 = (radio_count_msg_t*)call Packet.getPayload(&packet, sizeof(radio_count_msg_t));

	//time = (double)sim_time()/sim_ticks_per_sec();
	//dbg("TinyAMF", "time1=%f\n", time);

	//if (my_addr == DETECTOR && rcm->end_src == 9 && rcm->seq == 40) 
		dbg("RadioCountToLedsC", "[%f] Packet received (end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d)\n", time,  rcm->end_src, rcm->end_dest, rcm->src, rcm->dest, rcm->seq, rcm->payload);
	
	#if ROUTING==1
	if (my_addr == 3){
		goto end;
	}
	#endif

	block = 0;
	if (my_addr == DETECTOR){

		#if ROUTING==1
		routing_engine(rcm);
		#endif
		if (rcm->dest!=my_addr){
			goto end;
		}

		#if ATOMIC==1
 		block = block + atomic_engine(rcm);
		#endif
		
		#if MACE==1
		block = block + MAC_engine(rcm);
		#endif

		#if TIME==1
		block = block + time_correlation_engine(rcm);
		#endif

		#if SPACE==1
		save_packet(rcm);
		#endif

		#if FLOOD==1
		flood_engine(rcm);
		#endif

		//no_pack++;
		//dbg("TinyAMF", "no_pack=%hu\n",no_pack);
	}
	else
	if (rcm->dest!=my_addr){
		goto end;
	}

	//routing
	if (rcm->end_dest!=my_addr && block==0){
		
	
      		if (rcm1 == NULL) {
			return;
     	 	}

		rcm1->payload = rcm->payload;
		rcm1->src = my_addr;
		rcm1->dest = next_hop;
		rcm1->end_src = rcm->end_src;
		rcm1->end_dest = rcm->end_dest;
		rcm1->MAC = rcm->MAC;
		rcm1->seq = rcm->seq;
		
		#if SPACE==1
		rcm1->src_x = rcm->src_x;
		rcm1->src_y = rcm->src_y;
		#endif

		
		#if ROUTING==1
		if (my_addr== DETECTOR)
			routing_engine(rcm1);
		#endif

		//time = (double)sim_time()/sim_ticks_per_sec();
		//dbg("TinyAMF", "time2=%f\n", time);

		if (call AMSend.send(next_hop, &packet, sizeof(radio_count_msg_t)) == SUCCESS) {
			
			//dbg("RoutingLayer", "[%f] Routing packet (end_src=%hu end_dest=%hu src=%hu dest=%hu seq=%hu payload=%d)\n", time,  rcm1->end_src, rcm1->end_dest, rcm1->src, rcm1->dest, rcm1->seq, rcm1->payload);		
			locked = TRUE;//AM_BROADCAST_ADDR
	      	}
	}
	else{
		//if (my_addr == BS && rcm->end_src == 9 ) //&& rcm->seq==3
			//dbg("RadioCountToLedsC", "[%f] Packet received (end_src=%hu end_dest=%hu seq=%hu payload=%d)\n",  time, rcm->end_src, rcm->end_dest, rcm->seq, rcm->payload);
	}
	
	end:

      return bufPtr;
    }
  }

  event void AMSend.sendDone(message_t* bufPtr, error_t error) {
    if (&packet == bufPtr) {
      locked = FALSE;
	//time = (double)sim_time()/sim_ticks_per_sec();

	//dbg("TinyAMF", "time3=%f\n", time);
    }
  }

}




